using Unity.Mathematics;
using EarthUtility;
using Geometry;

namespace EarthGeospatial
{

    public class EllipsoidTangentPlane
    {

        public Ellipsoid pEllipsoid;
        public double3 Origin { get; set; }
        public double3 XAxis { get; set; }
        public double3 YAxis { get; set; }
        public Plane pPlane { get; set; }

        public double3 ZAxis=>this.pPlane.Normal;
        /**
         * @brief Creates a new instance.
         *
         * @param origin The origin, in cartesian coordinates.
         * @param ellipsoid The ellipsoid. Default value: {@link Ellipsoid::WGS84}.
         * @throws An `std::invalid_argument` if the given origin is at the
         * center of the ellipsoid.
         */
        public EllipsoidTangentPlane(double3 origin) : this(origin, Ellipsoid.WGS84) { }

        public EllipsoidTangentPlane(double3 origin, Ellipsoid ellipsoid) : this(computeEastNorthUpToFixedFrame(origin, ellipsoid), ellipsoid) { }
        /**
         * @brief Creates a new instance.
         *
         * @param eastNorthUpToFixedFrame A transform that was computed with
         * {@link Transforms::eastNorthUpToFixedFrame}.
         * @param ellipsoid The ellipsoid. Default value: {@link Ellipsoid::WGS84}.
         */
        public EllipsoidTangentPlane(double4x4 eastNorthUpToFixedFrame, Ellipsoid ellipsoid)
        {
            pEllipsoid = ellipsoid;
            Origin = eastNorthUpToFixedFrame[3].xyz;
            XAxis = eastNorthUpToFixedFrame[0].xyz;
            YAxis = eastNorthUpToFixedFrame[1].xyz;
            pPlane = new Plane(eastNorthUpToFixedFrame[3].xyz, eastNorthUpToFixedFrame[2].xyz);
        }

        public double2 projectPointToNearestOnPlane(double3 cartesian)
        {
            Ray ray = new Ray(cartesian, pPlane.Normal);
            double3? intersectionPoint = IntersectionTests.rayPlane(ray, this.pPlane);
            if (!intersectionPoint.HasValue)
            {
                intersectionPoint = IntersectionTests.rayPlane(-ray, this.pPlane);
                if (!intersectionPoint.HasValue)
                {
                    intersectionPoint = cartesian;
                }
            }

            double3 v = intersectionPoint.Value - this.Origin;
            return new double2(math.dot(this.XAxis, v), math.dot(this.YAxis, v));
        }

        public static double4x4 computeEastNorthUpToFixedFrame(double3 origin, Ellipsoid ellipsoid)
        {
            var scaledOrigin = ellipsoid.scaleToGeodeticSurface(origin);
            if (!scaledOrigin.HasValue)
            {
                throw new System.Exception("The origin must not be near the center of the ellipsoid.");
            }
            return Transforms.eastNorthUpToFixedFrame(scaledOrigin.Value, ellipsoid);
        }

    }

}